多机协同规划方案

[TOC]

本文档旨在说明如何在不同场景下使用ARAL算法库进行多机协同规划, 只讨论在同一个场景(Scene)下的规划方案。

本方案参考的接口文档

1. 联动与同步

• 联动 (Coordination): 指的是两个机器人在协同运动时，一个机器人的运动会引起另一个机器人的工具或者工件发生变化。例如，当一个工件被放置在旋转台上时，操作该工件的机器人需要根据旋转台的运动来调整自己的位姿。

  • 当两个联动的机器人操作同一个工件，并有刚性约束时，至少需要一个机器人处于柔顺控制模式。
  • 联动需要指定两个机器人的主从关系，在规划时，先确定主机器人的轨迹，然后解算从机器人的轨迹。

• 同步 (Synchronization): 指的是多个机器人在时序/时间/相位上需要保持一致，即运动在时空上有依赖关系。

2. 常见场景与实现方案

场景一: 不联动不同步

• 需求

  1. 两个机械臂同一个场景内独立执行任务，彼此之间没有运动关联。

• 实现方案:

  • 为每个机器人创建一个独立的Planner实例。
  • 分别为每个Planner添加运动指令（如tpAddPositionLine）。
  • 为每个Planner创建一个独立的Task实例来管理其生命周期。

• 示例代码:

  // 为机器人A创建独立的规划器和任务调度器
  auto planner_A_H = scene->rlCreateRobotPlanner({role_A}, "plan_A");
  auto planner_A = scene->rlGetRobotPlanner(planner_A_H);
  auto task_A = scene->rlCreateRobotTask({planner_A_H}, "task_A");
  
  auto planner_B_H = scene->rlCreateRobotPlanner({role_B}, "plan_B");
  auto planner_B = scene->rlGetRobotPlanner(planner_B_H);
  auto task_B = scene->rlCreateRobotTask({planner_B_H}, "task_B");
  
  // 两个规划器异步生成运动轨迹
  planner_A->addMoveLine(target_A);
  planner_B->addMoveArc(via_B, target_B);
  
  // 两个任务调度器异步采样
  task_A->tskUpdateCycle(cycle, point_A);
  task_B->tskUpdateCycle(cycle, point_B);
  

本文档中的示例代码是软件调用算法接口层的方式，不是用户层代码。

场景二: 联动不同步

• 需求:
  1. 机器人A(旋转变位机或者线性同步带)移动工件，机器人B需要控制工具在工件上移动，但它们的任务开始和结束时间不必同步。一般这种场景为轨迹跟踪，机器人B在工件上的速度需要考虑工艺要求。

• 实现方案:

  • 规划机器人B的工具在工件坐标系下的运动轨迹
  • 获取机器人A状态
  • 将机器人B的任务轨迹（例如，焊接、喷涂）与机器人A的轨迹进行叠加

• 示例说明:

  • 机器人B的任务是规划一个正方形轨迹。在每个规划周期，将正方形轨迹上的点与同一时刻机器人A的位姿进行叠加，得到最终的目标点，然后下发给机器人。

在ARAL中，这通常通过轨迹叠加（trajectory superposition）来实现。通常在软件层面通过周期性地更新目标点位来实现。也可以在算法层实现：

 // 具体分两种情况：
 // 1）机器人A独立规划，机器人B联动
 // 2) 机器人A由外部控制, 机器人B联动
  if(Independent){ // 为机器人A创建独立规划器
      auto planner_A_H = scene->rlCreateRobotPlanner({state_A, SLAVE}, "plan_A");
      auto planner_A = scene->rlGetRobotPlanner(planner_A_H);
  }
  // 为机器人B创建联动规划器(规划机器人B的工具在工件坐标系下的运动轨迹)
  if(Independent)
      auto planner_B_H = scene->rlCreateRobotPlanner({ {state_A, SLAVE}, {state_B, MASTER} }, "plan_B", );
  else
      auto planner_B_H = scene->rlCreateRobotPlanner({ {state_A, REFERENCE}, {state_B, MASTER} }, "plan_B", );
  auto planner_B = scene->rlGetRobotPlanner(planner_B_H);

  //创建联动任务管理器
  if(Independent)
      auto task_coord_H = scene->rlCreateRobotTask({planner_A_H, planner_B_H}, "task_B");
  else
     auto task_coord_H = scene->rlCreateRobotTask({planner_B_H}, "task_B");

  // 两个规划器生成联动轨迹
  if(Independent)
      planner_A->addMoveLine(target_A); 
  planner_B->addMoveArc(via_B, target_B);

  // 两个规划器被同一个任务模块调度
  if(Independent)
       task_coord->tskUpdateCycle(cycle, {point_A, point_B}); 
  else{
       state_A->rsUpdateJointPVA(q, qd, qdd);     // 如果机器人A由外部控制，则需要更新A的状态
       task_coord->tskUpdateCycle(cycle, {point_B});
  }

这种场景主要需要解决机器人B的运动约束问题，即需求获得机器人B的运动能力，机器人A的运动能力和工艺速度要求之间的平衡。

场景三: 同步不联动

• 需求

• 两个机器人协同完成一个任务，例如在一个工件上机器人A走直线，机器人B走圆弧(或者同时走moveJ, 运动过程中有碰撞风险)，要求它们的运动同启同停(或相位同步)。

• 两个机械臂同一个场景内独立执行任务，因为碰撞风险或者任务依赖，在时序上需要同步。(可以参考场景一方案, 时序由软件层逻辑控制解决)

• 实现方案:
  • 定义规划器: 创建两个Planner实例，并传入两个机器人的State句柄。
  • 统一任务管理: 创建一个Task来统一调度这两个Planner。

在ARAL中，如果两个机器人同时走moveJ，则会将两个机器人整合成一个机器人进行整体规划; 如果走轨迹，要求时间同步，通常通过时间缩放（time scaling）来实现; 如果需要相位同步，则在协调控制模块实现(需要抽象现有的相位同步逻辑，对此类场景，叠加两层相位同步）。

• 示例说明：机器人A和机器人B在同一个工件上操作，A走一段直线，B走一段直线和一条圆弧，需要A和B同启同停，示例代码如下：

  // 为机器人A/B创建独立的规划器
  auto planner_A_H = scene->rlCreateRobotPlanner({role_A}, "plan_A");
  auto planner_A = scene->rlGetRobotPlanner(planner_A_H);
  auto planner_B_H = scene->rlCreateRobotPlanner({role_B}, "plan_B");
  auto planner_B = scene->rlGetRobotPlanner(planner_B_H);
  
  // 为机器人A/B创建同步任务模块调度
  auto task_sync = scene->rlCreateRobotTask({planner_A_H, planner_B_H}, "task_sync");
  
  // 两个规划器生成联动轨迹
  task_sync->tskWaitSyncMove();
  planner_A->addMoveLine(target_A); 
  planner_B->addMoveLine(target_B); 
  planner_B->addMoveArc(via_B, target_C);
  task_sync->tskSyncMoveOn(); // planner_A的一条指令和planner_B的两条指令同步
  
  // 两个规划器被同一个任务模块调度
  task_sync->tskUpdateCycle(cycle, {point_A, point_B});
  

场景四: 联动同步

• 需求:

  1. 在场景二的基础上，要求两个机器人的运动同时开始、同时结束。

  2. 两个机器人协同完成一个任务，例如共同搬运一个物体

  3. 机械臂安装在导轨上进行大范围作业（如喷涂），要求导轨和机械臂的运动同启同停。

     以上三种需求可以具体分成两类情形

• 实现方案:

  1. 路径分解

     • 应用层将整体的作业路径分解为机器人A和机器人B的运动轨迹

     • 确定主从关系

     • 按照场景三中的方式进行规划

• 示例说明：机器人A和机器人B共同搬运一个重物，运动分解后，A走一段直线，B也走一段直线，需要A和B同启同停，示例代码如下：

 // 为机器人A/B创建独立的规划器
 auto planner_A_H = scene->rlCreateRobotPlanner({role_A}, "plan_A");
 auto planner_A = scene->rlGetRobotPlanner(planner_A_H);
 auto planner_B_H = scene->rlCreateRobotPlanner({role_B}, "plan_B");
 auto planner_B = scene->rlGetRobotPlanner(planner_B_H);

 // 为机器人A/B创建同步任务模块调度
 auto task_sync = scene->rlCreateRobotTask({planner_A_H, planner_B_H}, "task_sync");

 // 两个规划器生成联动轨迹
 task_sync->tskWaitSyncMove();
 planner_A->addMoveLine(target_A); 
 planner_B->addMoveLine(target_B); 
 task_sync->tskSyncMoveOn(); // planner_A的一条指令和planner_B的两条指令同步

 // 两个规划器被同一个任务模块调度
 task_sync->tskUpdateCycle(cycle, {point_A, point_B});

//!!! 需要注意的是，因为A/B机器人操作同一个工具，存在联动，所以A/B至少有一个需要是柔顺控制模式

1. 整体路径统一规划

   • 只规划一个整体路径(工具在工件中的运动轨迹）
   • 确定主从关系
   • 创建一个sync_planner实例，并将两个机器人的State句柄都传入进行统一规划
   • 协同路径规划: sync_planner内部会处理两个机器人的时间或者相位耦合。

2. 示例说明：机器人A(导轨)和机器人B共同喷涂一个工件，只关心工具在工件坐标系下的轨迹（如走一段直线），需要A和B同启同停，示例代码如下：

   // 为机器人A/B创建联动规划器(规划机器人B的工具在工件坐标系下的运动轨迹)
   auto planner_H = scene->rlCreateRobotPlanner({ {state_A, SLAVE}, {state_B, MASTER} }, "plan", );
   auto planner = scene->rlGetRobotPlanner(planner_H);
   
   // 创建联动任务管理器
   auto task_coord = scene->rlCreateRobotTask({planner}, "task");
   
   // 规划器生成联动轨迹
   planner->addMoveLine(target_B); 
   
   // 两个规划器被同一个任务模块调度
   task_coord->tskUpdateCycle(cycle, {point_A, point_B}); 
   
   //!! 上面的例子同样适用于两个机器人协同搬运，如只规划主机器人的轨迹，从机器人同步移动即可
   

3. 场景示例说明

ABB Fanta Bin 挑战，应用场景如下:

该场景包含3个子任务：

• 子任务1： 机器人2 进行独立运动，走确定性轨迹(如走圆弧)
• 子任务2： 机器人1的工具 在 机器人2 的末端托盘坐标系下走一个确定的轨迹(正常工具)
• 子任务3： 机器人3 控制它的末端托盘，使得机器人2的末端工具在机器人3的末端托盘坐标系下走确定的轨迹(外部工具)。

 // 为机器人1/2/3创建对应的规划器
 auto planner_2_H = scene->rlCreateRobotPlanner({state_2, MASTER}, "plan_2");
 auto planner_2 = scene->rlGetRobotPlanner(planner_2_H);

 auto planner_1_H = scene->rlCreateRobotPlanner({state_2, REFERENCE}, {state_1, MASTER}, "plan_1");
 auto planner_1 = scene->rlGetRobotPlanner(planner_1_H);

 auto planner_3_H = scene->rlCreateRobotPlanner({state_2, REFERENCE}, {state_3, MASTER}, "plan_3");
 auto planner_3 = scene->rlGetRobotPlanner(planner_3_H);

 // 为机器人1/2/3创建同步任务模块调度
 auto task_sync = scene->rlCreateRobotTask({planner_1_H, planner_2_H, planner_3_H}, "task_sync");

 // 3个规划器生成联动轨迹
 task_sync->tskWaitSyncMove();
 planner_2->addMoveLine(target_A); 
 planner_1->addMoveSpline(..., target_N); 
 planner_3->addMoveSpline(..., target_M); 
 task_sync->tskSyncMoveOn(); // planner_2/planner_1/planner_3的相关指令同步

 // 两个规划器被同一个任务模块调度
 task_sync->tskUpdateCycle(cycle, {point_1, point_2, point_3});

4. 总结